#include "foc.h"
#include "usr_arm_math.h"


motor __motor;

void set_ch_pwm(TIM_HandleTypeDef *htim,uint8_t ch, float time)
{
    if (ch <= 4)
    {
        switch (ch)
        {
        case 1:
            htim->Instance->CCR1 = (uint32_t)time;
            break;
        case 2:
            htim->Instance->CCR2 = (uint32_t)time;
            break;
        case 3:
            htim->Instance->CCR3 = (uint32_t)time;
            break;
        case 4:
             htim->Instance->CCR4 = (uint32_t)time;
        default: // if input a error channel will stop motor, this is not a safe action
            //stop motor???
            break;
        }
    }
}

void clark_trans(p_motor motor)
{
    motor->svpwm.ialpha = motor->svpwm.ia;
    motor->svpwm.ibeta = (motor->svpwm.ia + 2.0f * motor->svpwm.ib) / FOC_SQRT_3;
}

void park_trans(p_motor motor)
{
    motor->svpwm.id =  motor->svpwm.ialpha * __arm_cos_f32(motor->encoder.radian) + motor->svpwm.ibeta * __arm_sin_f32(motor->encoder.radian);
    motor->svpwm.iq = -motor->svpwm.ialpha * __arm_sin_f32(motor->encoder.radian) + motor->svpwm.ibeta * __arm_cos_f32(motor->encoder.radian);
}

void anti_park(p_motor motor, float id, float iq)
{
    motor->svpwm.ualpha = id * __arm_cos_f32(motor->encoder.radian) - iq * __arm_sin_f32(motor->encoder.radian);
    motor->svpwm.ubeta  = id * __arm_sin_f32(motor->encoder.radian) + iq * __arm_cos_f32(motor->encoder.radian);
}

void sector_judgment(p_motor motor)
{
    motor->svpwm.u1 = motor->svpwm.ubeta;
    motor->svpwm.u2 =  motor->svpwm.ualpha * FOC_SQRT3_2 - motor->svpwm.ubeta / 2.0f;
    motor->svpwm.u3 = -motor->svpwm.ualpha * FOC_SQRT3_2 - motor->svpwm.ubeta / 2.0f;

    uint8_t a = 0;//temp value
    uint8_t b = 0;
    uint8_t c = 0;
    uint8_t n = 0;
    
    a = (motor->svpwm.u1 > 0.0f) ? 1 : 0;
    b = (motor->svpwm.u2 > 0.0f) ? 1 : 0;
    c = (motor->svpwm.u3 > 0.0f) ? 1 : 0;
    n = 4 * c + 2 * b + a;
    switch(n)
    {
    case 1:
        motor->svpwm.sector = 2;
        motor->svpwm.tx =  SVPWM_SQRT3 * motor->svpwm.ts / motor->svpwm.udc * (-SVPWM_SQRT3_2 * motor->svpwm.ualpha + 0.5 * motor->svpwm.ubeta);
        motor->svpwm.ty =  SVPWM_SQRT3 * motor->svpwm.ts / motor->svpwm.udc * ( SVPWM_SQRT3_2 * motor->svpwm.ualpha + 0.5 * motor->svpwm.ubeta);
        break;
    case 2:
        motor->svpwm.sector = 6;
        motor->svpwm.tx =  SVPWM_SQRT3 * motor->svpwm.ts / motor->svpwm.udc * ( SVPWM_SQRT3_2 * motor->svpwm.ualpha + 0.5 * motor->svpwm.ubeta);
        motor->svpwm.ty = -SVPWM_SQRT3 * motor->svpwm.ts / motor->svpwm.udc * motor->svpwm.ubeta;
        break;
    case 3:
        motor->svpwm.sector = 1;
        motor->svpwm.tx =  SVPWM_SQRT3 * motor->svpwm.ts / motor->svpwm.udc * ( SVPWM_SQRT3_2 * motor->svpwm.ualpha - 0.5 * motor->svpwm.ubeta);
        motor->svpwm.ty =  SVPWM_SQRT3 * motor->svpwm.ts / motor->svpwm.udc * motor->svpwm.ubeta;
        break;
    case 4:
        motor->svpwm.sector = 4;
        motor->svpwm.tx = -SVPWM_SQRT3 * motor->svpwm.ts / motor->svpwm.udc * motor->svpwm.ubeta;
        motor->svpwm.ty =  SVPWM_SQRT3 * motor->svpwm.ts / motor->svpwm.udc * (-SVPWM_SQRT3_2 * motor->svpwm.ualpha + 0.5 * motor->svpwm.ubeta);
        break;
    case 5:
        motor->svpwm.sector = 3;
        motor->svpwm.tx =  SVPWM_SQRT3 * motor->svpwm.ts / motor->svpwm.udc * motor->svpwm.ubeta;
        motor->svpwm.ty = -SVPWM_SQRT3 * motor->svpwm.ts / motor->svpwm.udc * ( SVPWM_SQRT3_2 * motor->svpwm.ualpha + 0.5 * motor->svpwm.ubeta);
        break;
    case 6:
        motor->svpwm.sector = 5;
        motor->svpwm.tx = -SVPWM_SQRT3 * motor->svpwm.ts / motor->svpwm.udc * ( SVPWM_SQRT3_2 * motor->svpwm.ualpha + 0.5 * motor->svpwm.ubeta);
        motor->svpwm.ty = -SVPWM_SQRT3 * motor->svpwm.ts / motor->svpwm.udc * (-SVPWM_SQRT3_2 * motor->svpwm.ualpha + 0.5 * motor->svpwm.ubeta);  
        break;
    }
}


void pwm_duration_calc(p_motor motor)
{
    float ta = 0.0f;
    float tb = 0.0f;
    float tc = 0.0f;
    float td = 0.0f;
    switch(motor->svpwm.sector)
    {
    case 2://2 : 1+- -- ++
        ta = (motor->svpwm.ts + motor->svpwm.tx - motor->svpwm.ty) * 0.25f;
        tb = (motor->svpwm.ts - motor->svpwm.tx - motor->svpwm.ty) * 0.25f;
        tc = (motor->svpwm.ts + motor->svpwm.tx + motor->svpwm.ty) * 0.25f;
        break; 
    case 6: //6 : 2-- ++ +-
        ta = (motor->svpwm.ts - motor->svpwm.tx - motor->svpwm.ty) * 0.25f;
        tb = (motor->svpwm.ts + motor->svpwm.tx + motor->svpwm.ty) * 0.25f;
        tc = (motor->svpwm.ts + motor->svpwm.tx - motor->svpwm.ty) * 0.25f;
        break;
    case 1://1 : 3-- +- ++
        ta = (motor->svpwm.ts - motor->svpwm.tx - motor->svpwm.ty) * 0.25f;
        tb = (motor->svpwm.ts + motor->svpwm.tx - motor->svpwm.ty) * 0.25f;
        tc = (motor->svpwm.ts + motor->svpwm.tx + motor->svpwm.ty) * 0.25f;
        break;
    case 4://4 ++ +- --
        ta = (motor->svpwm.ts + motor->svpwm.tx + motor->svpwm.ty) * 0.25f;
        tb = (motor->svpwm.ts + motor->svpwm.tx - motor->svpwm.ty) * 0.25f;
        tc = (motor->svpwm.ts - motor->svpwm.tx - motor->svpwm.ty) * 0.25f;
        break;
    case 3://3 : 5 ++ -- +-
        ta = (motor->svpwm.ts + motor->svpwm.tx + motor->svpwm.ty) * 0.25f;
        tb = (motor->svpwm.ts - motor->svpwm.tx - motor->svpwm.ty) * 0.25f;
        tc = (motor->svpwm.ts + motor->svpwm.tx - motor->svpwm.ty) * 0.25f;
        break;
    case 5://5 : 6 +- ++ --
        ta = (motor->svpwm.ts + motor->svpwm.tx - motor->svpwm.ty) * 0.25f;
        tb = (motor->svpwm.ts + motor->svpwm.tx + motor->svpwm.ty) * 0.25f;
        tc = (motor->svpwm.ts - motor->svpwm.tx - motor->svpwm.ty) * 0.25f;
        break;
    default:
        break;
    }

    td = motor->svpwm.adc_ts; // not a low side current sample.
    
    motor->svpwm.ta = ta;
    motor->svpwm.tb = tb;
    motor->svpwm.tc = tc;
    motor->svpwm.td = td;
 
}


void motor_stop(void)
{
    __motor.func.set_ch_pwm(&SVPWM_TIMER, 1, 0.0f);
    __motor.func.set_ch_pwm(&SVPWM_TIMER, 2, 0.0f);
    __motor.func.set_ch_pwm(&SVPWM_TIMER, 3, 0.0f);
    __motor.status.enable = MOTOR_DIS;
}

void pwm_out(p_motor motor)
{
    if(MOTOR_EN == motor->status.enable)
    {
        motor->func.set_ch_pwm(&SVPWM_TIMER, 1, motor->svpwm.ta);
        motor->func.set_ch_pwm(&SVPWM_TIMER, 2, motor->svpwm.tb);
        motor->func.set_ch_pwm(&SVPWM_TIMER, 3, motor->svpwm.tc);
        motor->func.set_ch_pwm(&SVPWM_TIMER, 4, motor->svpwm.td);
    }
    else
    {
        motor_stop();
    }
    
}





